#include <stdio.h>
#include <time.h>
#include <unistd.h>
#include <wiringPi.h>

#define TRIG 0
#define ECHO 1

void send_trig_signal(){
	//send 5us low
	digitalWrite(TRIG,LOW);
	usleep(5);
	//send 10us high at least
	digitalWrite(TRIG,HIGH);
	usleep(11);
	//send low
	digitalWrite(TRIG,LOW);
}

int get_time_gap(struct timespec * start_time,struct timespec * end_time){
	int nsec = 0;

	while(!digitalRead(ECHO));
	clock_gettime(CLOCK_REALTIME,start_time);
	while(digitalRead(ECHO));
	clock_gettime(CLOCK_REALTIME,end_time);

	nsec = 1000000000*(end_time->tv_sec-start_time->tv_sec) + (end_time->tv_nsec-start_time->tv_nsec);

	return nsec;
}

void get_distance(const int* nsec){
	int ntime = *nsec;
	double dis = 0;
	dis = ntime/58000.0;
	printf("dis(cm):%f\n",dis);
}

int main(){
	int ntime_gap = 0;
	struct timespec start_time;
	struct timespec end_time;
	wiringPiSetup ();
	//set pinMode
	pinMode(TRIG,OUTPUT);
	pinMode(ECHO,INPUT);
	//send trigger signal
	send_trig_signal();
	//get the time gap
	ntime_gap = get_time_gap(&start_time,&end_time);

	//get the distance
	get_distance(&ntime_gap);

	return 0;
}
